/*
 * usonicRanger.cpp
 *
 *  Created on: Nov 22, 2012
 *      Author: bouchier
 */

#include <stdio.h>
#include <unistd.h>
#include "qegpioint.h"
#include "qetime.h"
#include "usonicRanger.h"


#define USPI 150
#define BIAS 300

extern CQEGpioInt &gpio;

static unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
{
  long val;

  val = ptv1->tv_usec - ptv0->tv_usec;
  val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;

  return val;
}

void usRangerCallback(unsigned int io, struct timeval *ptv, void *userData)
{
	int pw;
	struct usonicRanger *usranger_p;
	usranger_p = (struct usonicRanger *)userData;
	if (io == usranger_p->dioTrigIndex) {
		if (usranger_p->flag == 1) {			// if flag is still 1, the sense interrupt didn't fire
			usranger_p->distance = 1000;		// so set range to 1000
		} else {
			usranger_p->flag = 1;
		}
		usranger_p->tv0 = *ptv;
	}

	if (io == usranger_p->dioSenseIndex && usranger_p->flag) {
		usranger_p->flag = 0;
		pw = diff(&(usranger_p->tv0), ptv);
		if (pw > BIAS)
			usranger_p->distance = (pw - BIAS)/USPI;
		//printf("%d\n", usranger_p->distance);
	}
}

//! Trigger the ultrasonic ranger
/*
 * ACHTUNG - THIS ROUTINE IS SUBJECT TO CORRUPTION IF AN INTERRUPT THAT MANIPULATES GPIOS INTERRUPTS IT. I HOPE THAT
 * CAN'T HAPPEN
 */
void triggerUsonicRanger(struct usonicRanger *usRanger_p)
{
	int dioState;

	// pulse the trigger output high then low for 20us
	dioState = gpio.GetData();
	dioState |= (1<<(usRanger_p->dioTrigIndex));	// set the trigger
	gpio.SetData(dioState);
	CQETime::usleep(200);
	dioState &= ~(1<<(usRanger_p->dioTrigIndex));		// now clear it. This triggers the interrupt on trig pin
	gpio.SetData(dioState);
}

//! Initialize ultrasonic ranger
/*
 * Malloc the struct that tracks the ranger & set up the DIOs.
 * \param gpio The reference to the gpio object
 * \param dioTrigPort The VEXPro Digital I/O number that the sensor "IN" cable is connected to. This is the trigger
 * for starting a range measurement, and is driven high then low. The low-going transition initiates the
 * ultrasonic ping.
 * \param dioSensePort The VEXPro Digital I/O number that the sensor "OUT" cable is connected to. This is the
 * output which transitions low when the returning echo is receieved
 * \return pointer to the malloc'd control struct. Pass this as a handle to any sonar-related functions - it
 * is the object reference.
 */
struct usonicRanger * initUsonicRanger(CQEGpioInt& gpio, int dioTrigPort, int dioSensePort)
{
	// malloc the channel control struct & initialize it
	struct usonicRanger * usRanger_p = (struct usonicRanger *)malloc(sizeof(usonicRanger));
	usRanger_p->flag = 0;
	usRanger_p->distance = 0;
	usRanger_p->dioTrigIndex = dioTrigPort - 1;
	usRanger_p->dioSenseIndex = dioSensePort - 1;

	// set up the trigger DIO to be output & the Sense DIO to be an input & attach interrupt callback
	unsigned int dataDirection = gpio.GetDataDirection();
	dataDirection |= (1<<(usRanger_p->dioTrigIndex));		// Trigger is an output from VEXPro
	dataDirection &= ~(1<<(usRanger_p->dioSenseIndex));	// Sense is an input to VEXPro
	gpio.SetDataDirection(dataDirection);
	gpio.RegisterCallback(usRanger_p->dioTrigIndex, (void *)usRanger_p, usRangerCallback);
	gpio.RegisterCallback(usRanger_p->dioSenseIndex, (void *)usRanger_p, usRangerCallback);
	gpio.SetInterruptMode(usRanger_p->dioTrigIndex, QEG_INTERRUPT_NEGEDGE);
	gpio.SetInterruptMode(usRanger_p->dioSenseIndex, QEG_INTERRUPT_NEGEDGE);
	gpio.SetInterrupt(usRanger_p->dioTrigIndex, true);
	gpio.SetInterrupt(usRanger_p->dioSenseIndex, true);

	return usRanger_p;
}

